#include "eigen.h"

// ========== Vector3d计算 ==========
void vector3d_add(const Vector3d *a, const Vector3d *b, Vector3d *result)
{
    result->x = a->x + b->x;
    result->y = a->y + b->y;
    result->z = a->z + b->z;
}

void vector3d_sub(const Vector3d *a, const Vector3d *b, Vector3d *result)
{
    result->x = a->x - b->x;
    result->y = a->y - b->y;
    result->z = a->z - b->z;
}

double vector3d_dot(const Vector3d *a, const Vector3d *b)
{
    return a->x * b->x + a->y * b->y + a->z * b->z;
}

void vector3d_cross(const Vector3d *a, const Vector3d *b, Vector3d *result)
{
    result->x = a->y * b->z - a->z * b->y;
    result->y = a->z * b->x - a->x * b->z;
    result->z = a->x * b->y - a->y * b->x;
}

double vector3d_norm(const Vector3d *v)
{
    return sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
}

void vector3d_normalize(Vector3d *v)
{
    double norm = vector3d_norm(v);
    if (norm > 1e-12)
    {
        v->x /= norm;
        v->y /= norm;
        v->z /= norm;
    }
}

void vector3d_scale(const Vector3d *v, double scalar, Vector3d *result)
{
    result->x = v->x * scalar;
    result->y = v->y * scalar;
    result->z = v->z * scalar;
}

bool vector3d_is_zero(const Vector3d *v, double epsilon)
{
    double norm_sq = v->x * v->x + v->y * v->y + v->z * v->z;
    return norm_sq < (epsilon * epsilon);
}

// ==================== 3x3 矩阵操作实现 ====================
void matrix_multiply_3x3(const Matrix3d *a, const Matrix3d *b, Matrix3d *result)
{
    for (int i = 0; i < 3; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            double sum = 0;
            for (int k = 0; k < 3; ++k)
            {
                sum += a->data[i][k] * b->data[k][j];
            }
            result->data[i][j] = sum;
        }
    }
}

void matrix_transpose_3x3(const Matrix3d *a, Matrix3d *result)
{
    for (int i = 0; i < 3; ++i)
    {
        for (int j = 0; j < 3; ++j)
        {
            result->data[j][i] = a->data[i][j];
        }
    }
}

void matrix_inverse_3x3(const Matrix3d *a, Matrix3d *result)
{
    const double (*m)[3] = a->data;
    double det = m[0][0] * (m[1][1] * m[2][2] - m[1][2] * m[2][1]) - m[0][1] * (m[1][0] * m[2][2] - m[1][2] * m[2][0]) +
                 m[0][2] * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);

    if (fabs(det) < 1e-15)
    { // 奇异判断
        // 奇异矩阵,返回单位矩阵
        matrix3d_set_identity(result);
        return;
    }

    double inv_det = 1.0 / det;

    result->data[0][0] = (m[1][1] * m[2][2] - m[1][2] * m[2][1]) * inv_det;
    result->data[0][1] = -(m[0][1] * m[2][2] - m[0][2] * m[2][1]) * inv_det;
    result->data[0][2] = (m[0][1] * m[1][2] - m[0][2] * m[1][1]) * inv_det;
    result->data[1][0] = -(m[1][0] * m[2][2] - m[1][2] * m[2][0]) * inv_det;
    result->data[1][1] = (m[0][0] * m[2][2] - m[0][2] * m[2][0]) * inv_det;
    result->data[1][2] = -(m[0][0] * m[1][2] - m[0][2] * m[1][0]) * inv_det;
    result->data[2][0] = (m[1][0] * m[2][1] - m[1][1] * m[2][0]) * inv_det;
    result->data[2][1] = -(m[0][0] * m[2][1] - m[0][1] * m[2][0]) * inv_det;
    result->data[2][2] = (m[0][0] * m[1][1] - m[0][1] * m[1][0]) * inv_det;
}

void matrix3d_set_identity(Matrix3d *m)
{
    memset(m->data, 0, sizeof(m->data));
    m->data[0][0] = m->data[1][1] = m->data[2][2] = 1.0;
}

void matrix3d_mul_vector3d(const Matrix3d *m, const Vector3d *v, Vector3d *result)
{
    result->x = m->data[0][0] * v->x + m->data[0][1] * v->y + m->data[0][2] * v->z;
    result->y = m->data[1][0] * v->x + m->data[1][1] * v->y + m->data[1][2] * v->z;
    result->z = m->data[2][0] * v->x + m->data[2][1] * v->y + m->data[2][2] * v->z;
}

void matrix3d_from_array(Matrix3d *m, const double data[3][3])
{
    memcpy(m->data, data, sizeof(m->data));
}